#ifndef CALIB
#define CALIB
 
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdlib.h>
#include <sys/shm.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <vector>
#include <string>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include <tf/transform_broadcaster.h>
#include <time.h>
#include <sys/stat.h>
#include <sys/types.h> 
using namespace std;
using PointType = pcl::PointXYZI;

class Calib{
    public:
        Eigen::Matrix4f R0_rect_ = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f P2_ = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f Trv2c_ = Eigen::Matrix4f::Identity();
        pcl::PointCloud<PointType>::Ptr p_pc_frstum_;
        int img_h_ = 480;
        int img_w_ = 640;
        cv::Mat depth_img_;
    
    private:
        float trv2c_[12] = {0, -1, 0, 0.0, 
                                        0, 0, -1, -0.1377, 
                                        1, 0, 0, -0.3};
        float p2_[12] = {462.1379699707031, 0.0, 320.0, 0.0, 
            0.0, 462.1379699707031, 240.0, 0.0, 
            0.0, 0.0, 1.0, 0.0};

    public:
        Calib(){ 
            for (int i = 0; i<12; i++){
                P2_(i/4, i%4) = p2_[i];
                Trv2c_(i/4, i%4) = trv2c_[i];
            }
         }
          
        ~Calib() {
        }   

        void get_img_size_extrinsics(int img_w,int img_h,float * p2, float * trv2c){
            img_w_ = img_w;
            img_h_ = img_h;
            for(int i = 0; i<12; i++){
                P2_(i/4, i%4) = p2[i];
                Trv2c_(i/4, i%4) = trv2c[i];
            }
        }

        void get_frstum_point_idx_img(pcl::PointCloud<PointType>::Ptr p_pc, cv::Mat& pc_idx_img, cv::Mat& rgb_show){
            Eigen::Vector3f pt;
            Eigen::Vector4f pt_camera;
            for (int i = 0;i < p_pc->points.size(); i++){
                pt_camera = Trv2c_*Eigen::Vector4f(p_pc->points[i].x, p_pc->points[i].y, p_pc->points[i].z, 1);
                if (pt_camera(2) > 0 ){
                    pt = (P2_*pt_camera).topRows(3);
                    int32_t u = static_cast<int32_t>(pt(0)/pt(2));
                    int32_t v = static_cast<int32_t>(pt(1)/pt(2));
                    if (u>=0 && u< img_w_ && v >= 0 && v < img_h_ ){
                        circle(rgb_show, cv::Point(u,v), 1, cv::Scalar(int(pt(2)/10*255),0, int((10 - pt(2))/10*255)), 2);
                        pc_idx_img.at<float>(v, u ) = i;
                    }
                }
            }
        }

        Eigen::Vector4f img2lidar(int u, int v, float depth){
            Eigen::Vector4f pt;
            pt(0) = ((u - P2_(0, 2))*depth ) / P2_(0,0) + P2_(0,3) / (-P2_(0,0));
            pt(1) = ((v - P2_(1, 2))*depth ) / P2_(1,1) + P2_(1,3) / (-P2_(1,1));
            pt(2) = depth;
            pt(3) = 1;
            // pt = (Trv2c.inverse()*pt).topRows(3);
            pt = (Trv2c_.inverse()*pt);
            return pt;
        }
};

// class PcDataLoader{
//     public:
        
// }
#endif
